#include <math.h>
#include <ros/ros.h>
#include <std_msgs/UInt8.h>
#include <sensor_msgs/LaserScan.h>
#include <mavros_msgs/State.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Range.h>
#include <uav/position.h>

//*******************************全局变量*******************************

int flag[5]={0,0,0,0,0};

void tfmini_cb(const sensor_msgs::Range::ConstPtr& msg){
    if(msg->range)
    {
    	flag[0]=1;
    }
}

void odom_cb(const nav_msgs::Odometry::ConstPtr& msg){
    if(msg->pose.pose.position.z)
    {
        flag[1]=1;
    }
}


void target_cb(const uav::position::ConstPtr& msg){
    if(msg->z)
    {
        flag[2]=1;
    }    
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
    if(msg->angle_min)
    {
        flag[3]=1;
    }
}


void state_cb(const mavros_msgs::State::ConstPtr& msg){
    if(msg->connected)
    {
        flag[4]=1;
    }
}


//*******************************主函数********************************
int main(int argc, char **argv)
{
    ros::init(argc, argv, "test");
    ros::NodeHandle nh("~");


    
    //订阅和发布消息
    ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>
            ("/camera/odom/sample", 20, odom_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("/mavros/state", 50, state_cb);
    ros::Subscriber laser_sub = nh.subscribe<sensor_msgs::LaserScan>
            ("/scan", 50, scanCallback);
    ros::Subscriber target_sub = nh.subscribe<uav::position>
            ("/uav/target", 50, target_cb);
    ros::Subscriber tfmini_sub = nh.subscribe<sensor_msgs::Range>
            ("/mavros/distance_sensor/hrlv_ez4_pub", 20, tfmini_cb);
    ros::Publisher conneted_pub = nh.advertise<std_msgs::UInt8>
            ("/uav/connected", 20);

    std_msgs::UInt8 connected_info;
    int i=0; 
    int array[5]={2,3,5,7,11};          //利用质数分解
    
    ros::Rate rate(20);
    
    //发布各传感器连接状态消息
    while(ros::ok()){
	connected_info.data=1;
        for(i=0; i<5; i++){
            if (flag[i] == 0){
                connected_info.data *= array[i];
            }
        }
        conneted_pub.publish(connected_info);

        ros::spinOnce();
        rate.sleep();
	    
    } 
    return 0;

}


